import matplotlib.pyplot as plt
import math
import numpy as np

def sin_velocity_planner(current_velocity, end_velocity, s_length, s_size) -> (float, list):
    """
    基于加速度为sin曲线来计算速度规划，那么速度曲线为cos曲线（积分关系）
    @param current_velocity : 当前速度 km/h
    @param end_velocity : 规划终点速度 km/h
    @param s_length : frenet坐标下规划长度 m
    @param s_size : 速度序列长度
    @return max_acc : 最大加速度
    @return velocity_list : 速度序列
    """
    velocity_buffer = [current_velocity] * s_size
    max_acc = 0
    # k * cos(phy * 0) + bias = current_velocity
    # k * cos(phy * s_length) + bias = end_velocity
    # 令 phy * s_length = n * pi，即经过半个周期完成减速，
    # 因为 cos(0) = 1, cos(pi) = -1
    if current_velocity != end_velocity:
        bias = (current_velocity + end_velocity) / 2.0
        k = (current_velocity - end_velocity) / 2.0
        phy = np.pi / s_length
        s_buffer = np.linspace(0.0, s_length, num=s_size)
        velocity_buffer = k * np.cos(phy * s_buffer) + bias
        max_acc = k*k*phy/2.0
    return max_acc, velocity_buffer

if __name__ == "__main__":
    s_v = 10.0
    e_v = 0.0
    s_len = 10.0
    s_size = 50
    max_acc, v_buffer = sin_velocity_planner(s_v, e_v, s_len, s_size)
    print('max_acc = ', max_acc)
    print('last = ', v_buffer[-1])
    s_buffer = np.linspace(0.0, s_len, num=s_size)
    plt.plot(s_buffer, v_buffer, 'r')
    plt.show()